#ifndef ROS_UDP_COMM_LOW_HPP
#define ROS_UDP_COMM_LOW_HPP

#include <ros/ros.h>
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "stair_ctrl/UdpState.h"
#include <std_msgs/UInt8.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"

#include "stair_ctrl/convert.h"

#include <iostream>
#include <array>

using namespace UNITREE_LEGGED_SDK;

class rosUdpCommLowVilo
{
private:
    UDP low_udp;
    Safety safe;

    LowCmd low_cmd = {0};
    LowState low_state = {0};

    ros::Subscriber sub_low;
    ros::Subscriber sub_reset_state;

    ros::Publisher pub_low;
    ros::Publisher pub_udp_state;
    ros::Publisher pub_leg;
    ros::Publisher pub_imu;

    ros::WallTimer timer;

    unitree_legged_msgs::LowState low_state_ros;
    stair_ctrl::UdpState udp_state_ros;

public:
    rosUdpCommLowVilo(ros::NodeHandle *nh) : low_udp(LOWLEVEL, 8090, "192.168.123.10", 8007),
                                                                   safe(LeggedType::Go1)
    {
        auto hints = ros::TransportHints().tcpNoDelay();
        low_udp.InitCmdData(low_cmd);

        int state_rate;
        std::string low_cmd_topic, low_state_topic, udp_state_topic, reset_topic, leg_topic, imu_topic;
        nh->param<int>("state_rate", state_rate, 500);
        nh->param<std::string>("low_cmd_topic", low_cmd_topic, "/stair_ctrl/low_cmd");
        nh->param<std::string>("low_state_topic", low_state_topic, "low_state");
        nh->param<std::string>("udp_state_topic", udp_state_topic, "udp_state");
        nh->param<std::string>("reset_topic", reset_topic, "/stair_ctrl/reset");
        nh->param<std::string>("leg_topic", leg_topic, "/hardware_go1/joint_foot");
        nh->param<std::string>("imu_topic", imu_topic, "/hardware_go1/imu");

        pub_low = nh->advertise<unitree_legged_msgs::LowState>(low_state_topic, 100);
        pub_udp_state = nh->advertise<stair_ctrl::UdpState>(udp_state_topic, 100);
        sub_low = nh->subscribe(low_cmd_topic, 10, &rosUdpCommLowVilo::lowCmdCallback, this, hints);
        sub_reset_state = nh->subscribe(reset_topic, 10, &rosUdpCommLowVilo::resetCallback, this, hints);

        pub_leg = nh->advertise<sensor_msgs::JointState>(leg_topic, 100);
        pub_imu = nh->advertise<sensor_msgs::Imu>(imu_topic, 100);
        
        timer = nh->createWallTimer(ros::WallDuration(1.0 / state_rate), &rosUdpCommLowVilo::periodicCommCallback, this);
    }

    void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
    {
        low_cmd = rosMsg2Cmd(msg);

        low_udp.SetSend(low_cmd);
    }

    void resetCallback(const std_msgs::UInt8::ConstPtr &msg)
    {
        std::cout << "get reset code: " << (int)msg->data << std::endl;
        if (msg->data == 233)
        {
            low_udp.InitCmdData(low_cmd);
        }
    }

    void lowUdpCommUpdate()
    {
        low_udp.Send();
        low_udp.Recv();
        low_udp.GetRecv(low_state);
    }

    void lowUdpStateUpdate()
    {
        ros::Time now = ros::Time::now();

        low_state_ros = state2rosMsg(low_state);
        pub_low.publish(low_state_ros);

        udpStateToRosMsg();
        pub_udp_state.publish(udp_state_ros);

        sensor_msgs::Imu imu_msg;
        sensor_msgs::JointState joint_state_msg;

        imu_msg.header.stamp = now;
        imu_msg.angular_velocity.x = low_state.imu.gyroscope[0];
        imu_msg.angular_velocity.y = low_state.imu.gyroscope[1];
        imu_msg.angular_velocity.z = low_state.imu.gyroscope[2];
        imu_msg.linear_acceleration.x = low_state.imu.accelerometer[0];
        imu_msg.linear_acceleration.y = low_state.imu.accelerometer[1];
        imu_msg.linear_acceleration.z = low_state.imu.accelerometer[2];

        joint_state_msg.header.stamp = now;
        joint_state_msg.name = {"FL0", "FL1", "FL2",
                                "FR0", "FR1", "FR2",
                                "RL0", "RL1", "RL2",
                                "RR0", "RR1", "RR2",
                                "FL_foot", "FR_foot", "RL_foot", "RR_foot"};
        joint_state_msg.position.assign(16, 0);
        joint_state_msg.velocity.assign(16, 0);
        joint_state_msg.effort.assign(16, 0);
        for (int i = 0; i < 12; ++i)
        {
            joint_state_msg.position.at(i) = low_state.motorState.at(i).q;
            joint_state_msg.velocity.at(i) = low_state.motorState.at(i).dq;
        }
        for (int i = 0; i < 4; ++i)
        {
            joint_state_msg.effort.at(i + 12) = low_state.footForce.at(i);
        }

        pub_leg.publish(joint_state_msg);
        pub_imu.publish(imu_msg);
    }

    void udpStateToRosMsg()
    {
        udp_state_ros = stair_ctrl::UdpState();
        udp_state_ros.TotalCount = low_udp.udpState.TotalCount;
        udp_state_ros.SendCount = low_udp.udpState.SendCount;
        udp_state_ros.RecvCount = low_udp.udpState.RecvCount;
        udp_state_ros.SendError = low_udp.udpState.SendError;
        udp_state_ros.FlagError = low_udp.udpState.FlagError;
        udp_state_ros.RecvCRCError = low_udp.udpState.RecvCRCError;
        udp_state_ros.RecvLoseError = low_udp.udpState.RecvLoseError;
    }

    void periodicCommCallback(const ros::WallTimerEvent& event)
    {
        lowUdpCommUpdate();
        lowUdpStateUpdate();
    }

    void start()
    {
        ros::spin();
    }

    // void start(int state_rate)
    // {
    //     ros::Rate loop_rate(state_rate);

    //     // main loop
    //     while (ros::ok())
    //     {
    //         lowUdpCommUpdate();
    //         lowUdpStateUpdate();

    //         ros::spinOnce();
    //         loop_rate.sleep();
    //     }
    // }
};

#endif